#include <array>

class BoundaryConditionApplier {
public:
    // Main routine to apply boundary conditions
    void applyBoundaryConditions(
        const std::array<double, 9>& boundaryConditionMatrix, // BCS array
        std::array<double, 6>& accelerations,                // A array (1-3: translational, 4-6: rotational)
        int boundaryCode,                                    // ICODE
        const std::array<double, 9>& lambdaMatrix,          // BLAMB array
        bool isRigidBodyCOG,                               // IRBCOG flag
        bool hasLocalSkew                                  // ISKEW flag
    ) {
        // Extract boundary condition flags from the code
        auto [j1, j2, j3, j4, j5, j6] = extractBoundaryFlags(boundaryCode);

        if (!hasLocalSkew) {
            // Apply boundary conditions in global system
            applyGlobalBoundaryConditions(accelerations, j1, j2, j3);

            if (isRigidBodyCOG) {
                // Handle rigid body center of gravity transformations
                applyRigidBodyCOGConditions(accelerations, lambdaMatrix, j4, j5, j6);
            }
        } else {
            // Transform to local skew system, apply BCs, then transform back
            applySkewSystemBoundaryConditions(
                boundaryConditionMatrix, accelerations, 
                j1, j2, j3, j4, j5, j6, 
                lambdaMatrix, isRigidBodyCOG);
        }
    }

private:
    // Helper method to extract boundary condition flags from the code
    std::tuple<int, int, int, int, int, int> extractBoundaryFlags(int boundaryCode) {
        constexpr int LENNOD = 8; // Length of node number in bits (from NODE_COM)
        
        int ifix = boundaryCode >> LENNOD;
        int itrig = ifix >> 6;
        
        int j1 = ((ifix & 32) >> 5) * itrig;
        int j2 = ((ifix & 16) >> 4) * itrig;
        int j3 = ((ifix & 8) >> 3) * itrig;
        int j4 = ((ifix & 4) >> 2) * itrig;
        int j5 = ((ifix & 2) >> 1) * itrig;
        int j6 = (ifix & 1) * itrig;
        
        return {j1, j2, j3, j4, j5, j6};
    }

    // Apply boundary conditions in global coordinate system
    void applyGlobalBoundaryConditions(
        std::array<double, 6>& accelerations,
        int j1, int j2, int j3
    ) {
        accelerations[0] *= (1 - j1); // X translation
        accelerations[1] *= (1 - j2); // Y translation
        accelerations[2] *= (1 - j3); // Z translation
    }

    // Handle rigid body center of gravity transformations
    void applyRigidBodyCOGConditions(
        std::array<double, 6>& accelerations,
        const std::array<double, 9>& lambdaMatrix,
        int j4, int j5, int j6
    ) {
        // Transform rotational accelerations to global system
        double t1 = lambdaMatrix[0]*accelerations[3] + lambdaMatrix[3]*accelerations[4] + lambdaMatrix[6]*accelerations[5];
        double t2 = lambdaMatrix[1]*accelerations[3] + lambdaMatrix[4]*accelerations[4] + lambdaMatrix[7]*accelerations[5];
        double t3 = lambdaMatrix[2]*accelerations[3] + lambdaMatrix[5]*accelerations[4] + lambdaMatrix[8]*accelerations[5];
        
        // Apply boundary conditions in global system
        t1 *= (1 - j4);
        t2 *= (1 - j5);
        t3 *= (1 - j6);
        
        // Transform back to body system
        accelerations[3] = lambdaMatrix[0]*t1 + lambdaMatrix[1]*t2 + lambdaMatrix[2]*t3;
        accelerations[4] = lambdaMatrix[3]*t1 + lambdaMatrix[4]*t2 + lambdaMatrix[5]*t3;
        accelerations[5] = lambdaMatrix[6]*t1 + lambdaMatrix[7]*t2 + lambdaMatrix[8]*t3;
    }

    // Handle boundary conditions in local skew coordinate system
    void applySkewSystemBoundaryConditions(
        const std::array<double, 9>& boundaryConditionMatrix,
        std::array<double, 6>& accelerations,
        int j1, int j2, int j3, int j4, int j5, int j6,
        const std::array<double, 9>& lambdaMatrix,
        bool isRigidBodyCOG
    ) {
        // Transform translational accelerations to local skew system
        double t1 = boundaryConditionMatrix[0]*accelerations[0] + boundaryConditionMatrix[1]*accelerations[1] + boundaryConditionMatrix[2]*accelerations[2];
        double t2 = boundaryConditionMatrix[3]*accelerations[0] + boundaryConditionMatrix[4]*accelerations[1] + boundaryConditionMatrix[5]*accelerations[2];
        double t3 = boundaryConditionMatrix[6]*accelerations[0] + boundaryConditionMatrix[7]*accelerations[1] + boundaryConditionMatrix[8]*accelerations[2];
        
        // Apply boundary conditions in local system
        t1 *= (1 - j1);
        t2 *= (1 - j2);
        t3 *= (1 - j3);
        
        // Transform back to global system
        accelerations[0] = boundaryConditionMatrix[0]*t1 + boundaryConditionMatrix[3]*t2 + boundaryConditionMatrix[6]*t3;
        accelerations[1] = boundaryConditionMatrix[1]*t1 + boundaryConditionMatrix[4]*t2 + boundaryConditionMatrix[7]*t3;
        accelerations[2] = boundaryConditionMatrix[2]*t1 + boundaryConditionMatrix[5]*t2 + boundaryConditionMatrix[8]*t3;

        if (isRigidBodyCOG) {
            // Transform rotational accelerations to global system first
            double r1 = lambdaMatrix[0]*accelerations[3] + lambdaMatrix[3]*accelerations[4] + lambdaMatrix[6]*accelerations[5];
            double r2 = lambdaMatrix[1]*accelerations[3] + lambdaMatrix[4]*accelerations[4] + lambdaMatrix[7]*accelerations[5];
            double r3 = lambdaMatrix[2]*accelerations[3] + lambdaMatrix[5]*accelerations[4] + lambdaMatrix[8]*accelerations[5];
            
            // Transform to local skew system
            t1 = boundaryConditionMatrix[0]*r1 + boundaryConditionMatrix[1]*r2 + boundaryConditionMatrix[2]*r3;
            t2 = boundaryConditionMatrix[3]*r1 + boundaryConditionMatrix[4]*r2 + boundaryConditionMatrix[5]*r3;
            t3 = boundaryConditionMatrix[6]*r1 + boundaryConditionMatrix[7]*r2 + boundaryConditionMatrix[8]*r3;
            
            // Apply boundary conditions
            t1 *= (1 - j4);
            t2 *= (1 - j5);
            t3 *= (1 - j6);
            
            // Transform back to global system
            r1 = boundaryConditionMatrix[0]*t1 + boundaryConditionMatrix[3]*t2 + boundaryConditionMatrix[6]*t3;
            r2 = boundaryConditionMatrix[1]*t1 + boundaryConditionMatrix[4]*t2 + boundaryConditionMatrix[7]*t3;
            r3 = boundaryConditionMatrix[2]*t1 + boundaryConditionMatrix[5]*t2 + boundaryConditionMatrix[8]*t3;
            
            // Transform back to body system
            accelerations[3] = lambdaMatrix[0]*r1 + lambdaMatrix[1]*r2 + lambdaMatrix[2]*r3;
            accelerations[4] = lambdaMatrix[3]*r1 + lambdaMatrix[4]*r2 + lambdaMatrix[5]*r3;
            accelerations[5] = lambdaMatrix[6]*r1 + lambdaMatrix[7]*r2 + lambdaMatrix[8]*r3;
        }
    }
};